using BepuUtilities;
using System.Diagnostics;
using System.Runtime.CompilerServices;
#if MYCODE
using BepuUtilities.Vectors;
#else
using System.Numerics;
#endif

namespace BepuPhysics.Trees
{
    partial struct Tree
    {
        // TODO：没有很好的理由进行递归。这是原型机的残留物。
        unsafe void DispatchTestForNodeAgainstLeaf<TOverlapHandler>(int leafIndex, ref Vector3 leafMin, ref Vector3 leafMax, int nodeIndex, ref TOverlapHandler results) where TOverlapHandler : IOverlapHandler
        {
            if (nodeIndex < 0)
            {
                results.Handle(Encode(nodeIndex), leafIndex);
            }
            else
            {
                TestNodeAgainstLeaf(nodeIndex, leafIndex, ref leafMin, ref leafMax, ref results);
            }
        }
        private unsafe void TestNodeAgainstLeaf<TOverlapHandler>(int nodeIndex, int leafIndex, ref Vector3 leafMin, ref Vector3 leafMax, ref TOverlapHandler results) where TOverlapHandler : IOverlapHandler
        {
            ref var node = ref Nodes[nodeIndex];
            ref var a = ref node.A;
            ref var b = ref node.B;
            // 尽管进行了递归,Leaf Bound应该保持在L1中-它将从这里一直沿递归使用。
            // 然而,虽然我们可能在加载子A时加载子B,但不能保证它会一直存在。
            // 与将派生数据保留在堆栈上相比,在驱逐情况下重新加载它需要更多的工作。
            // TODO：这是一些非常有问题的微调。发现叶子之后的递归长度足以驱逐L1的情况并不常见。一定要测试一下。
            var bIndex = b.Index;
            var aIntersects = BoundingBox.Intersects(leafMin, leafMax, a.Min, a.Max);
            var bIntersects = BoundingBox.Intersects(leafMin, leafMax, b.Min, b.Max);
            if (aIntersects)
            {
                DispatchTestForNodeAgainstLeaf(leafIndex, ref leafMin, ref leafMax, a.Index, ref results);
            }
            if (bIntersects)
            {
                DispatchTestForNodeAgainstLeaf(leafIndex, ref leafMin, ref leafMax, bIndex, ref results);
            }
        }

        unsafe void DispatchTestForLeafAgainstNode<TOverlapHandler>(int leafIndex, ref Vector3 leafMin, ref Vector3 leafMax, int nodeIndex, ref Tree treeB, ref TOverlapHandler results) where TOverlapHandler : IOverlapHandler
        {
            if (nodeIndex < 0)
            {
                results.Handle(leafIndex, Encode(nodeIndex));
            }
            else
            {
                TestLeafAgainstNode(leafIndex, ref leafMin, ref leafMax, nodeIndex, ref treeB, ref results);
            }
        }
        unsafe void TestLeafAgainstNode<TOverlapHandler>(int leafIndex, ref Vector3 leafMin, ref Vector3 leafMax, int nodeIndex, ref Tree treeB, ref TOverlapHandler results)
            where TOverlapHandler : IOverlapHandler
        {
            ref var node = ref treeB.Nodes[nodeIndex];
            ref var a = ref node.A;
            ref var b = ref node.B;
            // 尽管进行了递归,Leaf Bound应该保持在L1中-它将从这里一直沿递归使用。
            // 然而,虽然我们可能在加载子A时加载子B,但不能保证它会一直存在。
            // 与将派生数据保留在堆栈上相比,在驱逐情况下重新加载它需要更多的工作。
            // TODO：这是一些非常有问题的微调。发现叶子之后的递归长度足以驱逐L1的情况并不常见。一定要测试一下。
            var bIndex = b.Index;
            var aIntersects = BoundingBox.Intersects(leafMin, leafMax, a.Min, a.Max);
            var bIntersects = BoundingBox.Intersects(leafMin, leafMax, b.Min, b.Max);
            if (aIntersects)
            {
                DispatchTestForLeafAgainstNode(leafIndex, ref leafMin, ref leafMax, a.Index, ref treeB, ref results);
            }
            if (bIntersects)
            {
                DispatchTestForLeafAgainstNode(leafIndex, ref leafMin, ref leafMax, bIndex, ref treeB, ref results);
            }
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        unsafe void DispatchTestForNodes<TOverlapHandler>(ref NodeChild a, ref NodeChild b, ref Tree treeB, ref TOverlapHandler results) where TOverlapHandler : IOverlapHandler
        {
            if (a.Index >= 0)
            {
                if (b.Index >= 0)
                {
                    GetOverlapsBetweenDifferentNodes(ref Nodes[a.Index], ref treeB.Nodes[b.Index], ref treeB, ref results);
                }
                else
                {
                    // 叶B与节点A。请注意,我们必须维护秩序;树B节点应该始终位于第二个插槽中。
                    TestNodeAgainstLeaf(a.Index, Encode(b.Index), ref b.Min, ref b.Max, ref results);
                }
            }
            else if (b.Index >= 0)
            {
                // 叶A与节点B。请注意,我们必须维护秩序;树B节点应该始终位于第二个插槽中。
                TestLeafAgainstNode(Encode(a.Index), ref a.Min, ref a.Max, b.Index, ref treeB, ref results);
            }
            else
            {
                // 两片叶子。
                results.Handle(Encode(a.Index), Encode(b.Index));
            }
        }

        private unsafe void GetOverlapsBetweenDifferentNodes<TOverlapHandler>(ref Node a, ref Node b, ref Tree treeB, ref TOverlapHandler results) where TOverlapHandler : IOverlapHandler
        {
            ref var aa = ref a.A;
            ref var ab = ref a.B;
            ref var ba = ref b.A;
            ref var bb = ref b.B;
            var aaIntersects = Intersects(aa, ba);
            var abIntersects = Intersects(aa, bb);
            var baIntersects = Intersects(ab, ba);
            var bbIntersects = Intersects(ab, bb);

            if (aaIntersects)
            {
                DispatchTestForNodes(ref aa, ref ba, ref treeB, ref results);
            }
            if (abIntersects)
            {
                DispatchTestForNodes(ref aa, ref bb, ref treeB, ref results);
            }
            if (baIntersects)
            {
                DispatchTestForNodes(ref ab, ref ba, ref treeB, ref results);
            }
            if (bbIntersects)
            {
                DispatchTestForNodes(ref ab, ref bb, ref treeB, ref results);
            }
        }


        public unsafe void GetOverlaps<TOverlapHandler>(ref Tree treeB, ref TOverlapHandler overlapHandler) where TOverlapHandler : struct, IOverlapHandler
        {
            if (leafCount == 0 || treeB.leafCount == 0)
                return;
            if (leafCount >= 2 && treeB.leafCount >= 2)
            {
                // 这两棵树都有完整的节点;我们可以使用一般情况。
                GetOverlapsBetweenDifferentNodes(ref Nodes[0], ref treeB.Nodes[0], ref treeB, ref overlapHandler);
            }
            else if (leafCount == 1 && treeB.leafCount >= 2)
            {
                // 树A是退化的;需要特殊情况。
                ref var a = ref Nodes[0];
                ref var b = ref treeB.Nodes[0];
                var aaIntersects = Intersects(a.A, b.A);
                var abIntersects = Intersects(a.A, b.B);
                if (aaIntersects)
                {
                    DispatchTestForNodes(ref a.A, ref b.A, ref treeB, ref overlapHandler);
                }
                if (abIntersects)
                {
                    DispatchTestForNodes(ref a.A, ref b.B, ref treeB, ref overlapHandler);
                }
            }
            else if (leafCount >= 2 && treeB.leafCount == 1)
            {
                // 树B是退化的;需要特殊情况。
                ref var a = ref Nodes[0];
                ref var b = ref treeB.Nodes[0];
                var aaIntersects = Intersects(a.A, b.A);
                var baIntersects = Intersects(a.B, b.A);
                if (aaIntersects)
                {
                    DispatchTestForNodes(ref a.A, ref b.A, ref treeB, ref overlapHandler);
                }
                if (baIntersects)
                {
                    DispatchTestForNodes(ref a.B, ref b.A, ref treeB, ref overlapHandler);
                }
            }
            else
            {
                Debug.Assert(leafCount == 1 && treeB.leafCount == 1);
                if (Intersects(Nodes[0].A, treeB.Nodes[0].A))
                {
                    DispatchTestForNodes(ref Nodes[0].A, ref treeB.Nodes[0].A, ref treeB, ref overlapHandler);
                }
            }
        }

    }
}
